#!/usr/bin/env python
# -*- coding: utf-8 -*-

import roslib
import rospy
from pokemon_server.srv import PoseState,PoseStateResponse
import time
import math
from nav_msgs.srv import *
from nav_msgs.msg import *
from geometry_msgs.msg import PoseStamped
from dest_info import *

# PoseState转化为PoseStateResponse
def to_response(pose_state,index):
    return PoseStateResponse(True,index,pose_state.position_x,pose_state.position_y,pose_state.position_z,pose_state.orientation_x,pose_state.orientation_y,pose_state.orientation_z,pose_state.orientation_w)

# 获取两点的欧式距离
def cal_dist(point1,point2):
    d1=point1.position_x-point2.position_x
    d2=point1.position_y-point2.position_y
    return math.sqrt(d1**2+d2**2)


# 获取两点间的最短路径的距离
def get_two_pose_dis(point1,point2):
    return cal_dist(point1,point2)

def callbackFun(pose_state):
    print("pose_State",pose_state)
    min=9999
    dest=None
    index=-1
    # 遍历未访问的节点，找到距离最小的
    for i,pose in enumerate(all_pose):
        if(get_state(i)==0):
            dist=get_two_pose_dis(pose_state,pose)
            if(dist<min):
                min=dist
                dest=pose
                index=i
    # 如果找到，返回这个点
    if dest!=None:
        change_state(index,1)
        return to_response(dest,index)
    # 如果未找到，遍历正在访问的节点，找到距离最小的
    for i,pose in enumerate(all_pose):
        if(get_state(i)==1):
            dist=get_two_pose_dis(pose_state,pose)
            if(dist<min):
                min=dist
                dest=pose
                index=i
    # 如果找到，返回这个点
    if dest!=None:
        return to_response(dest.pose_state,index)
    # 都未找到，说明已经结束
    print('index', index)
    ret=PoseStateResponse(False,index,0, 0, 0, 0, 0,0, 0)
    return ret

def scheduler_server():
    rospy.init_node('scheduler')
    s = rospy.Service('/scheduler', PoseState, callbackFun)
    rospy.spin()

if __name__ == "__main__":
    scheduler_server()